//
// Created by sukai on 4/1/23.
//

#ifndef CONTNAV_GLOBAL_PLANNER_MOVEACTION_H
#define CONTNAV_GLOBAL_PLANNER_MOVEACTION_H


#include <ros/ros.h>
#include<ros/package.h>
#include <iostream>
#include <signal.h>
#include <std_msgs/String.h>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/Twist.h>

#include <std_srvs/SetBool.h>

#include <nav_msgs/Path.h>
#include<math.h>

#include "mbf_msgs/MoveBaseAction.h"
#include "mbf_msgs/ExePathAction.h"
#include <actionlib/client/simple_action_client.h>

//路径规划
#include  <contnav_srvs/moveAction.h>
#include  <contnav_srvs/global_planner.h>

//#include <cmath>
//把有关这个contnav_msgs 消息的代码都注释了，我代码中没有整理；也没把中文注释，日志功能 数据库功能等无用代码去除，需要去除这些代码
//#include "contnav_msgs/Dictionaries.h"
//#include "contnav_msgs/DictionariesVector.h"
//#include "contnav_srvs/TABLE_CONTORLLER.h"
//日志
#include  "contnav_srvs/ContnavLogger.h"
#include  <thread>

//恢复地图
#include <std_srvs/Empty.h>


#include "../common/common.h"
#include  <mutex>

#include "../currencyinclude/potential_calculator.h"
#include "../currencyinclude/expander.h"
#include "../currencyinclude/dijkstra.h"
#include "../currencyinclude/astar.h"
#include "../currencyinclude/orientation_filter.h"
#include <tf2_ros/transform_listener.h>
#include <actionlib/server/simple_action_server.h>
#include <move_base_msgs/MoveBaseAction.h>

#include <dynamic_reconfigure/server.h>
#include <nav_core/base_local_planner.h>
#include <nav_core/base_global_planner.h>
#include <nav_core/recovery_behavior.h>
#include <geometry_msgs/PoseStamped.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_msgs/GetPlan.h>

#include "auto_lock.h"

//写入文件 writeEulerAnglesToFile
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <fstream>
#include <sys/stat.h>

//路路径优化
#include <algorithm>

#define RED     "\033[31m"      /* Red */
#define GREEN   "\033[32m"      /* Green */
#define WHITE   "\033[37m"      /* White */

//动态参数
//#include "contnav_global_planner/ContnavGlobalPlannerConfig.h"
//状态机
#include <functional> // 导入 functional 头文件
#include <unordered_map> // 导入 unordered_map 头文件
#include <string> // 导入 string 头文件
#include <boost/any.hpp> // 导入 boost any 头文件


#define contnav_debug(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::DEBUG,className,classLine,format, ##__VA_ARGS__ )

#define contnav_info(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::INFO,className,classLine,format, ##__VA_ARGS__ )
#define contnav_warn(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::WARN,className,classLine,format, ##__VA_ARGS__ )
#define contnav_error(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::ERROR,className,classLine,format, ##__VA_ARGS__ )
#define contnav_fatal(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::FATAL,className,classLine,format, ##__VA_ARGS__ )

using namespace std;
using namespace contnav::thread;
namespace contnav_moveAction{




//typedefs to help us out with the action server so that we don't hace to type so much
typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

using GotoCtrl = actionlib::SimpleActionClient<mbf_msgs::MoveBaseAction>;//点到点
using ExeCtrl = actionlib::SimpleActionClient<mbf_msgs::ExePathAction>;//给路径直接走



    // 导航状态机类
    class NavigationStateMachine {
    public:
        // 构造函数
        NavigationStateMachine(ros::NodeHandle &nh) : nh_(nh), current_state_("") {}

        //  NavigationStateMachine 类的 add_state 方法以接受参数的向量
        void add_state(const std::string &name, std::function<void(const std::vector<boost::any> &)> state_function) {
            state_functions_[name] = state_function;
        }

        //  NavigationStateMachine 类的 set_state 方法以存储参数向量
        int set_state(const std::string &name, const std::vector<boost::any> &params = {}) {
            current_state_ = name;
            state_params_[name] = params;
            return state_params_[name].size();
        }


        // 获取当前状态的成员函数
        std::string get_current_state() const {
            return current_state_;
        }
        //获取当前状态的参数
        bool get_current_state_param(int index,boost::any &age) const {
            if(index<state_params_.at(current_state_).size()){
                std::vector<boost::any> args = state_params_.at(current_state_);
                age =  args.at(index);
                return true;
            }else{
                return false;
            }
        }

        //  NavigationStateMachine 类的 execute 方法以传递参数向量
        void execute() {
            if (!current_state_.empty() && state_functions_.count(current_state_)) {
                // std::cout << "=======2.开始执行状态机 execute ==================状态机当前状态 " <<current_state_ << std::endl;
                state_functions_[current_state_](state_params_[current_state_]);
            }
        }

    private:
        ros::NodeHandle nh_; // 节点句柄
        std::string current_state_; // 当前状态的名称
        std::unordered_map<std::string, std::vector<boost::any>> state_params_;//增加多个参数传递,当前状态的参数
        std::unordered_map<std::string, std::function<void(const std::vector<boost::any> &)>> state_functions_; // 存储状态名和对应的函数

    };


    //start-------------求两条线段的交点--------------------------------
    class Point {
    public:
        double x, y;

        Point() {}

        Point(double x, double y) {
            this->x = x;
            this->y = y;
        }
    };

    class Line {
    public:
        double a, b, c;

        Line(Point p1, Point p2) {
            a = p2.y - p1.y;
            b = p1.x - p2.x;
            c = p2.x * p1.y - p1.x * p2.y;
        }

        Point getIntersection(Line other) {
            double det = a * other.b - other.a * b;
            if (det == 0) {
                // lines are parallel or coincident, no intersection 直线平行或重合，无交叉
                return Point();
            }
            double x = (other.b * c - b * other.c) / det;
            double y = (a * other.c - other.a * c) / det;
            return Point(x, y);
        }
    };




    //end---------------------------------------------
    //-start-----------------------------------------------
    class OnePlanToMap {//封装一条路径【储存像素】，包括起点，终点，路径点，路径长度
    public:
        //todo 1.起始点坐标世界坐标转像素坐标 [需要计算点数及t值]
          int world_i_start_x, world_i_start_y;//起始点坐标（像素）
        //todo 2.目标坐标 （像素）
          int world_i_goal_x, world_i_goal_y;
        // 两点之间的线段长度（像素）
        int  distance = 0;
        //todo ************************** 一阶贝塞尔曲线计算插值 *********** ========================================================*****************
        vector<Point> distance_plan;//路径点坐标（像素）[一阶贝塞尔曲线计算出来的两点的直线像素点坐标数据]
        OnePlanToMap() {}

        OnePlanToMap(unsigned int &world_i_start_x,unsigned int &world_i_start_y, unsigned int &world_i_goal_x,unsigned int &world_i_goal_y,int  &distance ,vector<Point> &distance_plan) {
            this->world_i_start_x = (int)world_i_start_x;
            this->world_i_start_y = (int)world_i_start_y;
            this->world_i_goal_x = (int)world_i_goal_x;
            this->world_i_goal_y = (int)world_i_goal_y;
            this->distance = distance;
            this->distance_plan = distance_plan;

        }
    };


    //-end--------------------------------------


    //-start-----------------------------------------------
    class MorPlanToMap {//封装多条路径【储存像素】，包括起点，终点，路径点，路径长度
    public:

        string plan_type;//类型
        int plan_id;;//当前路径id
        string plan_name;//当前路径name
        string ns;//当前路径 namespace
        std::vector<int> target_plan_ids;//当前路径行走结束后，可以到达的路径id
        vector<Point>  distance_plan_all_vector;//todo 保存所有路径点 像素坐标
        std::vector<int> plan_indexs;//当前路径在地图中的索引，//当前路径在地图中的索引,在行优先的一维地图中的索引
        std::vector<geometry_msgs::PoseStamped> current_plan;//当前路径，坐标直 world世界坐标
        int cost = POT_HIGH;//当前路径的代价,初始值无穷大
       MorPlanToMap() {}

        MorPlanToMap(string &plan_type,int &plan_id,string &plan_name, std::vector<int> &target_plan_ids,std::vector<geometry_msgs::PoseStamped> &current_plan,vector<Point>  &distance_plan_all_vector,std::vector<int> &plan_indexs) {
            this->plan_id =plan_id;//路径唯一id
            this->plan_name = plan_name;//路径名称
            this->target_plan_ids = target_plan_ids;//当前路径行走结束后，可以到达的路径id
            this->current_plan = current_plan;////规划好的当前路径，坐标直 world世界坐标
            this->plan_type = plan_type;//路径类型
            this->distance_plan_all_vector = distance_plan_all_vector;
            this->plan_indexs = plan_indexs;////当前路径在地图中的索引,在行优先的一维地图中的索引

        }
        MorPlanToMap(int &plan_id,string &plan_name, std::vector<int> &target_plan_ids,std::vector<geometry_msgs::PoseStamped> &current_plan,vector<Point>  &distance_plan_all_vector,std::vector<int> &plan_indexs) {
            this->plan_id =plan_id;//路径唯一id
            this->plan_name = plan_name;//路径名称
            this->target_plan_ids = target_plan_ids;//当前路径行走结束后，可以到达的路径id
            this->current_plan = current_plan;////规划好的当前路径，坐标直 world世界坐标
            this->distance_plan_all_vector = distance_plan_all_vector;
            this->plan_indexs = plan_indexs;////当前路径在地图中的索引,在行优先的一维地图中的索引

        }

        MorPlanToMap(string &plan_type,int &plan_id,string &plan_name, std::vector<int> &target_plan_ids,vector<Point>  &distance_plan_all_vector,std::vector<int> &plan_indexs) {
            this->plan_id =plan_id;//路径唯一id
            this->plan_name = plan_name;//路径名称
            this->target_plan_ids = target_plan_ids;
            this->plan_type = plan_type;//路径类型
            this->distance_plan_all_vector = distance_plan_all_vector;
            this->plan_indexs = plan_indexs;////当前路径在地图中的索引,在行优先的一维地图中的索引

        }
        //反转数据
        std::vector<int> reverseData(map<int, MorPlanToMap> &execute_map_plan) {
            // 倒序排序 distance_plan_all_vector, plan_indexs, current_plan
            std::reverse(distance_plan_all_vector.begin(), distance_plan_all_vector.end());
            std::reverse(plan_indexs.begin(), plan_indexs.end());
            std::reverse(current_plan.begin(), current_plan.end());

            // 重新组合 target_plan_ids
            std::vector<int> new_target_plan_ids;
            for (auto &other_map_plan : execute_map_plan) {
                if (other_map_plan.first != this->plan_id) {
                    for (int other_target_id : other_map_plan.second.target_plan_ids) {
                        if (other_target_id == this->plan_id) {
                            new_target_plan_ids.push_back(other_map_plan.second.plan_id);
                            break;
                        }
                    }
                }
            }
            return new_target_plan_ids;
        }


    };


    //-end--------------------------------------
    //曼哈顿距离封装
    class PointDistance {
    public:
        PointDistance(){};
        //index ：每个点在地图中的索引，start_distance：起点到当前路径所有点的曼哈顿距离,goal_distance：终点点到当前路径所有点的曼哈顿距离    ，point：当前点位终点最近的点，plan_id 路径id，plan_name路径名称，ns路径的命名空间
        PointDistance(int &index,int &distance_plan_all_point_index, double &start_distance,double &goal_distance,Point &point,int &plan_id,string &plan_name,string &ns) {
            this->index = index; //每个点在行有优先的地图中的索引
            this->start_distance = start_distance;////计算起点到当前路径所有点的曼哈顿距离
            this->goal_distance=goal_distance;///计算终点点到当前路径所有点的曼哈顿距离
            this->point=point;//当前点位最近的点
            this->distance_plan_all_point_index=distance_plan_all_point_index;//当前点在distance_plan_all_point, vector集合中的索引

            this->plan_id=plan_id; //路径id
            this->plan_name=plan_name;//plan_name路径名称
            this->ns=ns;//ns路径的命名空间

        }

        int  distance_plan_all_point_index;// 点在distance_plan_all_point, vector集合中的索引

        int index;//每个点在行有优先的地图中的索引
        double start_distance;//计算起点到当前路径所有点的曼哈顿距离。
        double goal_distance  ;//计算终点到当前路径所有点的曼哈顿距离。
        Point point; //当前点位终点最近的点 像素
        int plan_id;//当前路径id
        string plan_name;//当前路径name
        string ns;//当前路径 namespace

    };
    //-end--------------------------------------

    //todo -start-----------------------------------------------
    //储存点到点规划的路径，记录key：终点路径的路径索引id 对应的 规划的路径封装的数据结构
    class CurrentPathGoToTargetPath {
    public:
        CurrentPathGoToTargetPath(){};

        CurrentPathGoToTargetPath(std::vector<Point> &points,std::vector<geometry_msgs::PoseStamped> &plan) {
            this->plan_points=points;
            this->plan=plan;

        }

        std::vector<Point> plan_points;
        std::vector<geometry_msgs::PoseStamped> plan;

    };
    //-end--------------------------------------




    //-end--------------------------------------

    class moveAction {
    public:
        moveAction(tf2_ros::Buffer &tf,ros::NodeHandle &nh);

        ~moveAction();

        //多线程 处理状态机
        void thread_msg();

        //多线程 处理状态机
        void stopCmd_thread();

        //刷新数据
        void callBack_refreshdb(std_msgs::String data);

        //定时执行
        void updatefun(const ros::TimerEvent &time_event);

        // sql 查询字典数据
        void queryAllFromTABLE_DICTIONARES();

        //日志级别 日志内容
        bool saveLogFormat(string logLevel, string className, int classLine, char *format, ...);

//sql where 条件
        void whereSql(std::string Condition, std::string Key_i, std::string Symbol, std::string Value,
                      contnav_srvs::TABLE_CONTORLLER &tableContorllerService);

//sql 封装条件数据
        void mapTOcontnav_srvs(map <string, string> &dictionarieMap_request_value,
                               contnav_srvs::TABLE_CONTORLLER &tableContorllerService);

        bool callback_global_plannerFun(contnav_srvs::moveAction::Request &request,
                                        contnav_srvs::moveAction::Response &response);
        //恢复速度
        void restore();
        void restore2();
        void init();

        void run();

    private:
        bool isReturnPile_=true;//超过时间： 是否返回充电桩 false绕障碍,rue回桩
        bool isSuspend_=true; //true:暂停,false 绕障碍
        bool isOneWay_ =false; //  是否单向导航 true 单向导航，false 双向导航
        // true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
        bool isthresholdClosestPoint_ = false;
        bool isSendGoalPlanOverturn_ = false;//是否反转路径导航，true 反转路径导航，false 正常路径导航

        string local_plan_toptic_="/move_base_flex/TebLocalPlannerROS/local_plan";//局部路径tooic名称
        NavigationStateMachine state_machine_;//导航状态机
        //对障碍物进行处理，这里记录时间，如果时间超过一定时间，就认为做相应的处理
        ros::Time obstacle_detected_time_= ros::Time::now();

        //local planner 一直没数据，等待一定时间，就认为做相应的处理
        //ros::Time obstacle_locaplanner_time_= ros::Time::now();
        //double  locaplanner_wait_time_= 15; // 机器人到障碍物等待超时时间时间 秒

        //记录全局路径
       // mbf_msgs::ExePathGoal global_path_;//未走过的路径
       // mbf_msgs::ExePathGoal  traversed_path_;//添加到已走过的路径中

        //导航============================================Move_base_flex================================================================
        // Move_base_flex(mbf) 的 SimpleActionClient 对象
        //actionlib::SimpleActionClient<mbf_msgs::ExePathAction> ac_;  //ExeCtrl ac_;//给路径直接走
        std::shared_ptr<GotoCtrl> gc_;  //GotoCtrl gc_;//给点位直接走
        std::shared_ptr<ExeCtrl> ac_;  //ExeCtrl ac_;//给路径直接走
        bool stopCmdFig_= false;//发布速度停止指令

       // std::shared_ptr<ExeCtrl> ac_;
       // std::shared_ptr<GotoCtrl> gc_;

       // std::unique_ptr<GotoCtrl> goto_ctrl_;//点到点  goto_ctrl_(new GotoCtrl("move_base_flex/move_base")),
      //  std::unique_ptr<ExeCtrl> exe_ctrl_;//给路径直接走  exe_ctrl_(new ExeCtrl("move_base_flex/exe_path"))




        //==============================================================================================================

        tf2_ros::Buffer &tf_;

        //MoveBaseActionServer *as_;



        // costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;//move_base的成本图, planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);//全局规划地图
        // costmap_2d::Costmap2D* costmap_;   //planner_costmap_ros_->getCostmap()
        //costmap_2d::Costmap2DROS* costmap_ros_;
        int nx_, ny_, ns_; /**< size of grid, in pixels */
        mutex m_;
        ros::NodeHandle nh_;
        std::string map_frame_;
        //因为 move_base_flex中没有 move_base_simple/goal toptic，所以在这里需要制作一个与movebase中一致可用的 move_base_simple/goal
        ros::Publisher action_goal_pub_,cmd_vel_pub_,goal_pub_;
        ros::Subscriber sub_, goal_sub_;


        //todo 全局地图
        ros::Subscriber costmap_sub_;
        ros::Subscriber costmap_updated_sub_;



        nav_msgs::OccupancyGrid grid_;
        std::atomic_bool grid_ready_;
        std::atomic_bool grid_updated_ready_;//更新一个全局大地图
        std::shared_ptr<costmap_2d::Costmap2D> origin_costmap_;
        std::string frame_id_;
        unsigned int cx_, cy_;
        tf2_ros::Buffer buffer2;









        //get some parameters that will be global to the move base node
        std::string global_planner_name_, local_planner_name_;
        bool  allow_unknown_;
        //    1451    private_nh2.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
        double planner_window_x_, planner_window_y_, default_tolerance_;
        int publish_scale_;
        bool outline_map_;
        string robot_base_frame_ = "base_link";//机器人底盘坐标系
        int markerId = 0;//多路径发布 markerId
        bool publish_potential_ = false;//true 发布代价地图
        unsigned char lethal_cost_ = 253;
        unsigned char neutral_cost_ = 66;
        bool unknown_ = false;//是否探索未知区域，flase--不可到达
        //日志 ,恢复代价地图,cmd_vel_client_暂停速度
        ros::ServiceClient contnavLoggerClient,clear_costmaps_client_,cmd_vel_client_;
        //数据库操作
        ros::ServiceClient SqlOperationClient,my_global_plannerClient;
        ros::ServiceServer global_plannerServer;
        //刷新数据库,订阅局部路径规划
        ros::Subscriber subscriber_refreshdb,local_path_sub_;
        ros::Timer timer2_;

        //字典数据
        map <string, string> dictionariesMap_;
        string logFileName = "global_planner.log";
//        Index *open_set_array_;
//        std::vector<Index> open_set_; //待遍历的点
//        std::vector<Index> queue_;
        //这段代码按照行优先的顺序迭代potential_数组的每个元素，并使用std::cout输出其值。表达式i * ny + j计算了在展开的数组中(i, j)元素的索引。每行末尾的std::endl确保输出按照每行一个的方式打印。
        //需要注意的是，由于potential_数组有384 x 384 = 147,456个元素，因此输出可能非常大。你可以将输出重定向到文件中，或者将其管道传递给分页程序（例如less），以便更轻松地查看。
        float *potential_; //地图数组存代价直

       //PotentialCalculator *p_calc_;
       // Expander *planner_ = NULL;
        boost::mutex mutex_;//自动解锁
        boost::mutex mutex1_;//自动解锁
        boost::mutex mutex2_;//自动解锁

        //使用： 自动解锁
        Mutex m_mutex;

        //1.封装一条路径【储存像素】，包括起点，终点，路径点，路径长度  [计算两条线段交点处的圆弧路径使用]
        vector <OnePlanToMap> onePlanToMaps_vector_;

        //参数服务器
       // boost::recursive_mutex configuration_mutex_;
        //dynamic_reconfigure::Server<contnav_global_planner::ContnavGlobalPlannerConfig> *dsrv_;



        string move_base_global_toptic_ = "/move_base_flex/global_costmap/costmap";// 订阅全局地图   /move_base_flex/global_costmap/costmap  /move_base/global_costmap/costmap
        string move_base_global_updates_toptic_ = "/move_base_flex/global_costmap/costmap_updates";//    /move_base_flex/global_costmap/costmap_updates  /move_base/global_costmap/costmap_updates



        string fileOutPut_ = "/home/sukai/workspace/workspace_ros_car_noetic/src/contnav_global_planner/output";//可配置路径




       // OrientationFilter *orientation_filter_;//计算方向

    private://feedbackCb中增加其它功能：1.检查机器人是否卡住。2.当机器人到达特定位置时执行其他操作（例如发布一条消息）。
        //geometry_msgs::PoseStamped  goal_pose_;//目标点位置
        geometry_msgs::PoseStamped last_pose_; // 上一次的位置
        ros::Publisher custom_pub_; // 发布一条消息
        bool stuck_; // 检查机器人是否卡住
        double position_diff_threshold_ = 0.01; // 检查机器人是否卡住的阈值
        double distance_threshold_ = 0.5; // 到达目标的距离阈值

        double min_distance_to_obstacle_ = 1; // 机器人到障碍物的最小距离
        double  wait_time_= 15; // 机器人到障碍物等待超时时间时间 秒
        double   current_distance_threshold_=0.1;// 如果距离小于一个阈值（例如0.1米），则认为机器人已经经过这个点

        // 点到点导航结果回调函数
        void doneCb(const actionlib::SimpleClientGoalState &state, const mbf_msgs::MoveBaseResultConstPtr &result);

        void activeCb();
        // 在此处处理导航过程中的反馈
        void feedbackCb(const mbf_msgs::MoveBaseFeedbackConstPtr &feedback);
        //---
        // 路径导航结果回调函数
        void
        resultCallbackExe(const actionlib::SimpleClientGoalState &state, const mbf_msgs::ExePathResultConstPtr &result);
        void activeCbExe();
        // 在此处处理导航过程中的反馈
        void feedbackCbExe(const mbf_msgs::ExePathFeedbackConstPtr &feedback);





        //订阅一个全局大地图，做障碍和珊格地图的提取
        void grid_callback(nav_msgs::OccupancyGridConstPtr const &msg);

        void grid_updated_callback(map_msgs::OccupancyGridUpdateConstPtr const &msg);

        auto grid2costmap(nav_msgs::OccupancyGrid const &grid) const -> std::shared_ptr<costmap_2d::Costmap2D>;

        // auto costmap2cv_mat(std::shared_ptr<costmap_2d::Costmap2D> const& costmap,int32_t x,int32_t y,int32_t width,int32_t height) -> cv::Mat;

        //清除成本图中的起始单元格，因为我们知道它不会成为障碍
        void clearRobotCell(const geometry_msgs::PoseStamped &global_pose, unsigned int mx, unsigned int my);

        /**
     * 具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
     * 然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。

    在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
     于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
     * @param costarr
     * @param nx
     * @param ny
     * @param value
     */
        void outlineMap(unsigned char *costarr, int nx, int ny, unsigned char value);


        //计算起点在一维数组中的下标。// 将起点坐标 start_x 和 start_y 转化为一维数组中的索引 start_i。
        int toIndex(int x, int y, int nx, int ny);

        //某个位置在字符数组中的索引为 index，则该位置在地图中的 current_x 和 current_y 像素坐标可以通过以下公式计算：
        void toX_Y(int index, int &current_x, int &current_y, int nx, int ny);

        void setSize(int nx, int ny);

//获取小车当前位置
        bool getRobotPose(geometry_msgs::PoseStamped &global_pose);

        bool getuwb_tf(string &pframe_id, string &cframe_id, geometry_msgs::TransformStamped &ps_out);




        //像素坐标转世界坐标
        void mapToWorld(double mx, double my, double &wx, double &wy);

/**
 *
 * @param wx
 * @param wy
 * @param mx
 * @param my
 这个函数的实现比较简单，主要包括以下几个步骤：

    检查给定的世界坐标 (wx, wy) 是否在 Costmap2D 对象覆盖的区域内，如果不在，则返回 false。

    将给定的世界坐标 (wx, wy) 转换为相应的离散地图坐标 (mx, my)。

    检查转换后的地图坐标 (mx, my) 是否在 Costmap2D 对象的有效范围内，如果超出，则返回 false。

    如果以上检查都通过了，则返回 true。

下面是对这个函数中各个参数的说明：

    wx 和 wy：待转换的世界坐标。
    mx 和 my：转换后的离散地图坐标。像素坐标
    origin_x_ 和 origin_y_：地图坐标系原点在世界坐标系中的位置。
    resolution_：地图的分辨率，即一个像素代表的实际距离。
    size_x_ 和 size_y_：地图的大小，即像素数。

需要注意的是，以上实现中使用了 C++ 中的类型转换语句 static_cast，将浮点数转换为整数类型，同时使用引用类型 & 将结果返回给调用者。
 */

        bool worldToMap(double wx, double wy, double &mx, double &my);















        //  半结构化路径规划traffic_goal
        void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);













        //写入文件
        void writeEulerAnglesToFile(const std::vector<geometry_msgs::PoseStamped> &plan, const std::string &filename);

        //创建文件路径
        void createDirectory(const std::string &path);

        //读取文件
        std::vector<geometry_msgs::PoseStamped> readEulerAnglesFromFile(const std::string &filename);
        //发布路径
        bool  sendGoalPlan(geometry_msgs::PoseStamped &robot_pose,geometry_msgs::PoseStamped &goal,mbf_msgs::ExePathGoal &result_goal_path);
        //翻转路径
       bool   sendGoalPlanOverturn(geometry_msgs::PoseStamped &robot_pose, geometry_msgs::PoseStamped &goal,mbf_msgs::ExePathGoal &result_goal_path);
        //接收机器人的当前位置，并将其与全局路径上的点进行比较。如果机器人当前位置与全局路径上的某个点的距离小于一个阈值（例如0.1米），我们认为机器人已经经过这个点。然后将这个点从未走过的路径中删除，并添加到已走过的路径中。
       // void updatePaths( geometry_msgs::PoseStamped &current_pose);
        //订阅局部路径规划
        void localPathCallback(const nav_msgs::Path::ConstPtr& local_path);

         //状态机
        // 调用 MBF 客户端导航至目标点
       void navigate_to_goalpath(const std::vector<boost::any> &args);
//
//        // 切换到监控状态,监控是否遇到障碍物，遇到就停止
        void monitor_progress(const std::vector<boost::any> &args);
//
        // 调用 MBF 客户端导航至目标点 (翻转路径)
       void navigate_to_goalpathOverturn(const std::vector<boost::any> &args);
        // 调用 MBF 客户端导航至目标点 (翻转路径)
        void navigate_to_goalpathDefaultgoal(const std::vector<boost::any> &args);
        //给充电桩发送充电指令
        void chargeFun(const std::vector<boost::any> &args);
        //获取数据 boost::any 中的数据，函数的输入参数包括 boost::any 对象和其期望的类型，函数的返回值是类型转换后的值
        template<typename T> T any_cast(const boost::any & operand);

        //判断map判断key是否存在
        template<typename KeyType, typename ValueType>
        bool containsKey(const std::map<KeyType, ValueType>& myMap, const KeyType& keyToCheck);


        //end=================路径优化===========================


        //----------------------
    };

}



#endif //CONTNAV_GLOBAL_PLANNER_MOVEACTION_H
